#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/LaserScan.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <geometry_msgs/TransformStamped.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl_ros/transforms.h>
#include <pcl/point_types.h>
#include <pcl/filters/passthrough.h>

class PointCloudToLaserScan
{
public:
  PointCloudToLaserScan()
    : tf_listener_(tf_buffer_)
  {
    ros::NodeHandle private_nh("~");

    // 参数读取
    private_nh.param("pointcloud_topic_in", pointcloud_topic_in_, std::string("/cloud_registered"));
    private_nh.param("laserscan_topic_out", laserscan_topic_out_, std::string("/scan"));
    private_nh.param("min_height_offset", min_height_offset_, 0.1);
    private_nh.param("max_height_offset", max_height_offset_, 1.0);
    private_nh.param("angle_min", angle_min_, -3.14159);
    private_nh.param("angle_max", angle_max_, 3.14159);
    private_nh.param("angle_increment", angle_increment_, 0.005);
    private_nh.param("scan_time", scan_time_, 10.0);
    private_nh.param("range_min", range_min_, 0.0);
    private_nh.param("range_max", range_max_, 30.0);
    private_nh.param("use_inf", use_inf_, true);
    private_nh.param("inf_epsilon", inf_epsilon_, 1.0);
    private_nh.param("if_debug", if_debug_, false);

    // 订阅与发布
    cloud_sub_ = nh_.subscribe(pointcloud_topic_in_, 10, &PointCloudToLaserScan::pointCloudCallback, this);
    scan_pub_ = nh_.advertise<sensor_msgs::LaserScan>(laserscan_topic_out_, 10);
    if (if_debug_)
    {
      debug_pub_ = nh_.advertise<sensor_msgs::PointCloud2>("/point_filted", 10);
      ROS_INFO("Debug mode enabled: publishing filtered clouds on /point_filted");
    }
  }

private:
  void pointCloudCallback(const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
  {
    // 获取当前高度
    double current_height = 0.0;
    auto timeStamp = cloud_msg->header.stamp;
    try
    {
      auto tf_stamped = tf_buffer_.lookupTransform("map", "body", timeStamp, ros::Duration(0.1));
      current_height = tf_stamped.transform.translation.z;
    }
    catch (const tf2::TransformException &ex)
    {
      ROS_WARN("TF lookup failed: %s", ex.what());
      return;
    }

    double min_height = current_height - min_height_offset_;
    double max_height = current_height + max_height_offset_;

    // 转换并滤波
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromROSMsg(*cloud_msg, *cloud);

    pcl::PassThrough<pcl::PointXYZ> pass;
    pass.setInputCloud(cloud);
    pass.setFilterFieldName("z");
    pass.setFilterLimits(min_height, max_height);
    pcl::PointCloud<pcl::PointXYZ>::Ptr filtered_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pass.filter(*filtered_cloud);

    // 调试模式下发布滤波后点云
    if (if_debug_)
    {
      sensor_msgs::PointCloud2 debug_msg;
      pcl::toROSMsg(*filtered_cloud, debug_msg);
      debug_msg.header = cloud_msg->header;
      debug_pub_.publish(debug_msg);
    }

    // 将点云从 map 坐标系变换到 body 坐标系
    pcl::PointCloud<pcl::PointXYZ>::Ptr transformed_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    if (!pcl_ros::transformPointCloud("body", *filtered_cloud, *transformed_cloud, tf_buffer_))
    {
      ROS_WARN("PointCloud transform to body frame failed");
      return;
    }

    // 生成 LaserScan
    sensor_msgs::LaserScan scan_msg;
    scan_msg.header.stamp = cloud_msg->header.stamp;
    scan_msg.header.frame_id = "body";
    scan_msg.angle_min = angle_min_;
    scan_msg.angle_max = angle_max_;
    scan_msg.angle_increment = angle_increment_;
    scan_msg.time_increment = 0.0;
    scan_msg.scan_time = scan_time_;
    scan_msg.range_min = range_min_;
    scan_msg.range_max = range_max_;

    size_t num_readings = std::ceil((scan_msg.angle_max - scan_msg.angle_min) / scan_msg.angle_increment);
    scan_msg.ranges.assign(num_readings, use_inf_ ? std::numeric_limits<float>::infinity() : range_max_ + inf_epsilon_);

    for (const auto& pt : transformed_cloud->points)
    {
      double angle = std::atan2(pt.y, pt.x);
      if (angle < angle_min_ || angle > angle_max_) continue;

      double range = std::hypot(pt.x, pt.y);
      // if (range < range_min_ || range > range_max_) continue;

      int idx = static_cast<int>((angle - angle_min_) / angle_increment_);
      if (idx >= 0 && idx < static_cast<int>(scan_msg.ranges.size()))
      {
        scan_msg.ranges[idx] = std::min(scan_msg.ranges[idx], static_cast<float>(range));
      }
    }

    scan_pub_.publish(scan_msg);
  }

  ros::NodeHandle nh_;
  ros::Subscriber cloud_sub_;
  ros::Publisher scan_pub_;
  ros::Publisher debug_pub_;
  tf2_ros::Buffer tf_buffer_;
  tf2_ros::TransformListener tf_listener_;

  std::string pointcloud_topic_in_;
  std::string laserscan_topic_out_;
  bool if_debug_;

  double min_height_offset_;
  double max_height_offset_;
  double angle_min_;
  double angle_max_;
  double angle_increment_;
  double scan_time_;
  double range_min_;
  double range_max_;
  bool use_inf_;
  double inf_epsilon_;
};

int main(int argc, char** argv)
{
  ros::init(argc, argv, "dynamic_height_pointcloud_to_laserscan");
  PointCloudToLaserScan node;
  ros::spin();
  return 0;
}
